#include "include.h"
#include "LQ_GPIO_KEY.h"
#include "LQ_TFT18.h"

// 锥桶数据结构
typedef struct {
    double latitude;
    double longitude;
    uint8_t index;
} ConePosition;

#define MAX_CONES 4
#define KEY_START KEY0
#define KEY_SAVE KEY1
#define KEY_STOP KEY2

static ConePosition g_cones[MAX_CONES];
static uint8_t g_current_cone = 0;
static uint8_t g_collection_mode = 0;

// PID控制器定义
static pid_param_t speed_pid;
static pid_param_t angle_pid;

void Adjust_Speed(float target_speed) {
    static float current_speed = 0;
    
    // 初始化PID参数
    if(speed_pid.kp == 0) {
        speed_pid.kp = 0.5f;
        speed_pid.ki = 0.1f;
        speed_pid.kd = 0.2f;
        speed_pid.imax = 1000;
    }
    
    // 计算PID输出
    float error = target_speed - current_speed;
    float pid_output = PidLocCtrl(&speed_pid, error);
    
    // 转换为电机控制信号
    BLDC_duty = (int)(8000 * constrain_float(pid_output, -1.0f, 1.0f));
    LQ_BLDCCtrl();
    
    // 更新当前速度估计值
    current_speed += pid_output * 0.01f; // 假设采样周期为10ms
}

void Adjust_Direction(float target_angle) {
    static float current_angle = 0;
    short gyrox, gyroy, gyroz;
    ICM_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
    
    // 初始化PID参数
    if(angle_pid.kp == 0) {
        angle_pid.kp = 0.8f;
        angle_pid.ki = 0.05f;
        angle_pid.kd = 0.3f;
        angle_pid.imax = 1000;
    }
    
    // 使用陀螺仪Z轴数据作为当前角度变化率
    float angle_rate = gyroz * 0.001f;
    current_angle += angle_rate;
    
    // 计算PID输出
    float error = target_angle - current_angle;
    float pid_output = PidLocCtrl(&angle_pid, error);
    
    // 转换为舵机PWM占空比
    uint32 servo_duty = 850 + (uint32)(pid_output * 10);
    ServoCtrl(ServoALL, servo_duty);
}

// 按键处理函数
void Handle_Keys(void) {
    static uint8_t last_state[3] = {1,1,1};
    uint8_t current_state;
    char displayText[50];
    
    // 开始采集
    current_state = KEY_Read(KEY_START);
    if(current_state == LOW && last_state[0] == HIGH) {
        g_collection_mode = 1;
        g_current_cone = 0;
        sprintf(displayText, "Collection started");
        Display_showString(0, 0, displayText, U16_BLACK, U16_WHITE, 16);
    }
    last_state[0] = current_state;
    
    // 保存当前位置
    current_state = KEY_Read(KEY_SAVE);
    if(current_state == LOW && last_state[1] == HIGH && g_collection_mode) {
        if(g_current_cone < MAX_CONES) {
            Get_DoubleData(&Save_Data, &g_cones[g_current_cone].longitude, &g_cones[g_current_cone].latitude);
            g_cones[g_current_cone].index = g_current_cone;
            g_current_cone++;
            sprintf(displayText, "Cone %d saved", g_current_cone);
            Display_showString(0, 1, displayText, U16_BLACK, U16_WHITE, 16);
        }
    }
    last_state[1] = current_state;
    
    // 结束采集
    current_state = KEY_Read(KEY_STOP);
    if(current_state == LOW && last_state[2] == HIGH && g_collection_mode) {
        g_collection_mode = 0;
        EEPROM_Write(0, 0, (unsigned long*)g_cones, sizeof(g_cones)/sizeof(unsigned long));
        sprintf(displayText, "Collection completed");
        Display_showString(0, 2, displayText, U16_BLACK, U16_WHITE, 16);
    }
    last_state[2] = current_state;
}

// 八字绕锥算法
void FigureEightAlgorithm(void) {
    double current_lon, current_lat;
    Get_DoubleData(&Save_Data, &current_lon, &current_lat);
    
    // 计算与各锥桶的距离
    for(int i=0; i<MAX_CONES; i++) {
        double dx = g_cones[i].longitude - current_lon;
        double dy = g_cones[i].latitude - current_lat;
        double distance = sqrt(dx*dx + dy*dy);
        
        // 根据距离调整速度和方向
        if(distance < 0.0001) { // 接近锥桶
            // 执行绕锥动作
            Adjust_Speed(0.5);
            Adjust_Direction(i % 2 == 0 ? 30 : -30);
        }
    }
}

// 主控制函数
void ConeNavigation_Main(void) {
    GPIO_KEY_Init();
    Display_Init();
    //位置信息读取
    EEPROM_Read(0, 0, (unsigned long*)g_cones, sizeof(g_cones)/sizeof(unsigned long));
    
    while(1) {
        parseGpsBuffer();
        Handle_Keys();
        
        if(!g_collection_mode && g_current_cone == MAX_CONES) {
            FigureEightAlgorithm();
        }
        
        Delay_Ms(10);
    }
}
